/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "voslam_blas.h"

/* gcc -O2 -Wall -g -c -o voslam_blas.o voslam_blas.c */

int choldcb(double *R,double *A, int N, int p)
{
    int i,j,k,l,lambda;
    double ir;
    

			
    for (j=0;j<N;j++)
    {

	lambda=j+p;
	if(lambda>N)
		lambda=N;
	lambda=lambda-j;
/*
	for (i=0;i<lambda;i++)
		R[i+j*p]=A[i+j*p];
*/
	k=j-p;
	if(k<0)
		k=0;	
        for (;k<j;k++)
        {
	    l=k+p;	
	    if(l>N)
		l=N;
	    l=l-j;
            for (i=0;i<l;i++) R[i+j*p]=R[i+j*p]-R[i+(j-k)+k*p]*R[(j-k)+k*p];
        }
        if(R[j*p]<=0)
	    return 0;
	ir=1./sqrt(R[j*p]);
        for (i=0;i<lambda;i++) R[i+j*p]=R[i+j*p]*ir;
        
    }
    return 1;
}

void cholslb(double *A, double *b, double *x, int N, int p)
{
    int i,k,stop;
    double sum;
    
    for (i=0;i<N;i++)
    { 
    	stop=i-p+1;
	if(stop<0)
		stop=0;
        for (sum=b[i],k=i-1;k>=stop;k--) sum -= A[(i-k)+k*p]*x[k];
        x[i]=sum/A[i*p]; /*BOOGIE*/
    }
    for (i=N-1;i>=0;i--)
    {
    	stop=p;
	if(i+p>N)
		stop=N-i;
        for (sum=x[i],k=1;k<stop;k++) sum -= A[k+i*p]*x[i+k];
        x[i]=sum/A[i*p];/*BOOGIE*/
    }
    
}


int choldc(double *R,double *A, int N)
{
    int i,j,k;
    double sum,ir=0;;
    
    for(i=N-1;i>=0;i--)
		for(j=i;j>=0;j--)
			R[i*N+j]=A[i*N+j];
			
    for (i=0;i<N;i++)
    {
        for (j=i;j<N;j++)
        {
            for (sum=R[i+j*N],k=i-1;k>=0;k--) sum -= R[i+k*N]*R[j+k*N];
            
            if (i == j)
            {
                if (sum <= 0.0) 
                    return 0;
                R[i+i*N]=sqrt(sum);
		ir=1./R[i+i*N];
            }
            else R[j+i*N]=sum*ir;
        }
    }
    return 1;
}
void cholsl(double *A, double *b, double *x, int N)
{
    int i,k;
    double sum;
    
    for (i=0;i<N;i++)
    { 
        for (sum=b[i],k=i-1;k>=0;k--) sum -= A[i+k*N]*x[k];
        x[i]=sum/A[i+i*N];
    }
    for (i=N-1;i>=0;i--)
    {
        for (sum=x[i],k=i+1;k<N;k++) sum -= A[k+i*N]*x[k];
        x[i]=sum/A[i+i*N];
    }
}
double find_largest_value(double *v,int N)
{
	double max;
	int i;
	
	max=v[0];
	for(i=1;i<N;i++)
	{
		if(v[i]>max)
		    max=v[i];
	}
	
	return max;
}

void back_substitution(double *A, double *b, double *x, int N)
{
    int i,k;
    double sum;

    for (i=N-1;i>=0;i--)
    {
        for (sum=b[i*N],k=i+1;k<N;k++) sum -= A[i+k*N]*x[k*N];
        x[i*N]=sum/A[i+i*N];
    }
}
double power_iter(double *A,double *v, double *x,int N)
{
	double max,nor,cor;
	int i,j;
	
	for(i=0;i<N;i++)
	{
		x[i]=A[i*N]*v[0];
	    for(j=1;j<N;j++)
	    {
		    x[i]+=A[i*N+j]*v[j];
	    }	
	}
	nor=0;
	cor=0;
	for(i=0;i<N;i++)
	{
	    cor+=v[i]*x[i];
	    nor+=v[i]*v[i];
	}
	max=find_largest_value(x,N);
	for(i=0;i<N;i++)
	    v[i]=x[i]/max;
	return cor/nor;
}

int calc_qr_light(double *A,double *R,int N)
{
    double sum,ir;
    int i,j,k;
    
    for(i=0;i<N;i++)
    {
        sum=A[i*N]*A[i*N];
        for (j=1;j<N;j++) sum += A[j+i*N]*A[j+i*N];
        
        if(sum<0.000000000001)
            return 0;
        R[i+i*N]=sqrt(sum);
        ir=1./R[i+i*N];
        for (j=0;j<N;j++) A[j+i*N]=A[j+i*N]*ir;

        for (k=i+1;k<N;k++)
        { 
            sum = A[i*N]*A[k*N];
            for (j=1;j<N;j++) sum += A[j+i*N]*A[j+k*N];
	    
            R[i+k*N]=sum;
            for (j=0;j<N;j++) A[j+k*N]= A[j+k*N]-A[j+i*N]*R[i+k*N];
        }
    }
    return 1;
}
void house(double *x,double *v,int n)
{
    double mu,beta;
    int i;
    
    mu=x[0]*x[0];
    for(i=1;i<n;i++)
        mu+=x[i]*x[i];
    mu=sqrt(mu);
    if(x[0]>0)
        beta=x[0]+mu;
    else
        beta=x[0]-mu;
    for(i=1;i<n;i++)
        v[i]=x[i]/beta;
     v[0]=1;
}
void left_householder_transform(double *R,double *v,double *w, int M, int N,int stride)
{
    double sum;
    int i,j;
    
    sum=v[0]*v[0];
    for(i=1;i<M;i++)
    sum+=v[i]*v[i];
//  printf("R=[");
    for(i=0;i<N;i++)
    {
        w[i]=v[0]*R[i*stride]/sum;
        for(j=1;j<M;j++)
        {
            w[i]+=v[j]*R[i*stride+j]/sum;
        }
    }
    for(i=0;i<N;i++)
    {
        for(j=0;j<M;j++)
        {
            R[i*stride+j]=R[i*stride+j]-2*v[j]*w[i];
//          printf(" %f",R[i*stride+j]);
        }
//      printf("\n");
    }
//      printf("]\n");
    
}
void right_householder_transform(double *Q,double *P,double *v,double *w, int M, int n)
{
    double sum;
    int i,j,k;
    
    sum=v[0]*v[0];
    for(i=1;i<(M-n);i++)
    sum+=v[i]*v[i];
//  printf("Q=[");
    for(i=0;i<M-n;i++)
    {
        for(j=0;j<M-n;j++)
        {
            w[i*(M-n)+j]=-2*v[j]*v[i]/sum;
        }
    }
    for(i=0;i<M-n;i++)
            w[i*(M-n)+i]=w[i*(M-n)+i]+1.;
    for(i=0;i<n;i++)
        for(j=0;j<M;j++)
            Q[i*M+j]=P[i*M+j];
    
            
    for(i=n;i<M;i++)
    {
        for(j=0;j<M;j++)
        {
            Q[i*M+j]=P[n*M+j]*w[(i-n)*(M-n)];
            for(k=n+1;k<M;k++)
            Q[i*M+j]+=P[k*M+j]*w[(i-n)*(M-n)+k-n];
//          printf(" %f",Q[i*M+j]);
        }
//      printf("\n");
    }
    for(i=n;i<M;i++)
    {
        for(j=0;j<M;j++)
        {
            P[i*M+j]=Q[i*M+j];
        
    //      printf(" %f",Q[i*M+j]);
        }
    //  printf("\n");
    }
    //printf("]\n");
}
void householder_matrix(double *P,double *v, int N)
{
    double sum;
    int i,j;
    
    sum=v[0]*v[0];
    for(i=1;i<N;i++)
    sum+=v[i]*v[i];
    
    for(i=0;i<N;i++)
    {
        for(j=0;j<N;j++)
        {
            P[i*N+j]=-2*v[j]*v[i]/sum;
        }
    }
    for(i=0;i<N;i++)
    {
        P[i*N+i]+=1.;
    }
    
}
void calc_qr(double *Q, double *P, double *R, double *v,double *w,int M,int N)
{
    int i;
 //   printf("x: %lf %lf %lf %lf %lf %lf %lf %lf %lf \n",R[0],R[1],R[2],R[3],R[4],R[5],R[6],R[7],R[8]);
    house(R,v,M);
//    printf("v: %lf %lf %lf %lf %lf %lf %lf %lf %lf \n",v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]);
    householder_matrix(P,v,M);
    left_householder_transform(R,v,w,M,N,M);
    for(i=1;i<N;i++)
    {
        house(R+i*M+i,v,M-i);
        left_householder_transform(R+i*M+i,v,w,M-i,N-i,M);
    //  printf("v: %f %f %f \n",v[0],v[1],v[2]);
    //  printf("w: %f %f %f \n",w[0],w[1],w[2]);
        right_householder_transform(Q,P,v,w,M,i);
    }

}

void find_smallest_eigenvalue(double *A,double *Q,double *R,double *iA, double *v, double *x, int N,int cyc)
{
    double max,min;
	int i;
	
	for(i=0;i<N*N;i++)
           Q[i]=A[i]; 
    calc_qr_light(Q,R,N);
    min=fabs(R[0]);
    for(i=1;i<N;i++)
    {
	    if(fabs(R[i*N+i])<min)
	        min=fabs(R[i*N+i]);
    }
    
    if(min<1e-8)
    {
        for(i=0;i<N*N;i++)
            Q[i]=A[i];
           
        for(i=0;i<N;i++)
	        Q[i*N+i]=Q[i*N+i]+1e-6;

            calc_qr_light(Q,R,N);
    }
  
    for(i=0;i<N;i++)
        back_substitution(R, Q+i, iA+i,N);
  
    for(i=0;i<N;i++)
        v[i]=iA[i];
  
    max=find_largest_value(x,N);
    for(i=0;i<N;i++)
	    v[i]=v[i]/max;
	 
    for(i=0;i<cyc;i++)
        power_iter(iA,v,x,N);
        
}

void horner(double *a, double *b, double x, int N)
{
	int i;
	
	b[0]=a[0];
	for(i=1;i<N;i++)
	{
		b[i]=a[i]+x*b[i-1];
	}
}
void complex_horner(double *a, double *b, complex x, int N)
{
	double r,u;
	int i;
	
	r=-2.*x.real;
	u=x.real*x.real+x.imag*x.imag;
	b[0]=a[0];
	b[1]=a[1]-r*a[0];
	for(i=2;i<N-1;i++)
	{
		b[i]=a[i]-r*b[i-1]-u*b[i-2];
	}
}

complex initial_madsen_root(double *a,double *a1, int N)
{
	complex v;
	double r,min,val,theta,p0,p1;
	int i;
	r=log(fabs(a[N-1]));
	
	min=fabs(a[N-1])/fabs(a[N-2]);
	for(i=N-3;i>0;i--)
	{
		if(a[i]!=0)
		{
		    val=exp((r-log(fabs(a[i])))/(N-i-1));
		    if(val<min)
		        min=val;
		}
	}
	p0=evaluate_poly(a,0,N);
	p1=evaluate_poly(a1,0,N-1);
	theta=atan2(-p0,p1);
	
	v.imag=min*0.5*sin(theta);
	v.real=min*0.5*cos(theta);
	
	return v;
}
void solve_quadratic(complex *x1, complex *x2,double *a)
{
    double D;

    D=a[1]*a[1]-4*a[0]*a[2];
    if(D<0)
    {
        x1->real=-a[1]/(2.*a[0]);
        x2->real=-a[1]/(2.*a[0]);
        x1->imag=sqrt(-D)/(2.*a[0]);
        x2->imag=-sqrt(-D)/(2.*a[0]);
    }
    else
    {
        x1->real=-(a[1]+sqrt(D))/(2.*a[0]);
        x2->real=-(a[1]-sqrt(D))/(2.*a[0]);
        x1->imag=0.;
        x2->imag=0.;
    }
}

void find_positive_real_roots(double *a,double *a1, double *b, double *x, int N,int cyc,int *k)
{
	complex v,v1,f;
	double u;
	int i,j,l,p;
	
	for(i=0;i<N;i++)
	    b[i]=a[i];

	j=0;

	for(i=0;i<N;i++)
	{
		if((N-i)==3)
		{
			solve_quadratic(&v,&v1,b);
			if(v.imag==0.)
			{
			    if(v.real>0)
			    {
					x[j]=v.real;
			        j++;
			    }
			    if(v1.real>0)
			    {
					x[j]=v1.real;
			        j++;
			    }	
			}
			break;
		}
		if((N-i)==2)
		{
			x[j]=-b[1]/b[0];
			if(x[j]>0)
			    j++;
			break;
		}
		for(l=0;l<N-1-i;l++)
	        a1[l]=b[l]*(N-1-l-i);
		v=initial_madsen_root(b,a1,N-i);
 //       printf("vi: %f %f\n",v.real,v.imag);   			
	    complex_next_root(b,a1, &v, &p,N-i,cyc);
//	    printf("v: %f %f\n",v.real,v.imag);
	    u=complex_evaluate_poly(a,v,&f,N);
	    if(u>1e-14)
	    {
	//		printf("u: %f\n",u);
	        break;
	    }
	    if(fabs(evaluate_poly(a,v.real,N))<5*1e-15)
	    {
			
			horner(b,b,v.real,N-i);
	        if(v.real>0)
	        {
				x[j]=v.real;
	            j++;
	        }
		}
		else
		{	    
	        complex_horner(b,b,v,N-i);
	        i++;
	    }

	}

	(*k)=j;
}

int complex_next_root(double *a,double *a1, complex *v, int *p,int N,int cyc)
{
    double d,u,du,oldu,ou,fac;
    complex p0,p1,x,t,dz,f,oldt;
    int i,k,cnt;
    
    t=(*v);
    cnt=1;
    for(i=0;i<cyc;i++)
    {
        ou=complex_evaluate_poly(a,t,&p0,N);
        du=complex_evaluate_poly(a1,t,&p1,N-1);
       
        if((fabs(p0.real)<2*1e-16)&&(fabs(p0.imag)<2*1e-16))
		{
			(*v)=t;
	        return 1;
		}
		if(ou==0)
		{
			(*v)=t;
	        return 1;
		}
		if(du==0)
		{
			t.real=t.real+0.5*p0.real;
		    t.imag=t.imag+0.5*p0.imag;
		    continue;
		}   
		dz.real=(p0.real*p1.real+p0.imag*p1.imag)/du;
        dz.imag=(p0.imag*p1.real-p1.imag*p0.real)/du;
 
		x.real=t.real-dz.real;
		x.imag=t.imag-dz.imag;
		oldu=complex_evaluate_poly(a,x,&f,N);
		if(oldu<ou)
		{
		
			t=x;
		    (*p)=1;
		    for(k=2;k<N;k++)
		    {
		        x.real=t.real-dz.real;
		        x.imag=t.imag-dz.imag;
		        u=complex_evaluate_poly(a,x,&f,N);
		        if(u>oldu)
		        {
				    break;
			    }
			    t=x;
			    oldu=u;
			    (*p)=k;
		    }
		}
		else
		{
			(*p)=-1;
			d=0.5;
            oldt=t;
		    for(k=1;k<N;k++)
		    {
		        x.real=oldt.real-d*dz.real;
		        x.imag=oldt.imag-d*dz.imag;
		        u=complex_evaluate_poly(a,x,&f,N);
		        if(u>oldu)
		        {
				    break;
			    }
			    t=x;
			    d=0.5*d;
			    oldu=u;

			    (*p)=-k;
		    }
		    if(oldu>ou)
		    {
				fac=1./cnt;
				t.real=oldt.real+fac*(-0.8*dz.real-0.6*dz.imag);
		        t.imag=oldt.imag+fac*(-0.8*dz.imag-0.6*dz.imag);
		        cnt++;
			}
			
		}
	//	printf("ou: %f oldu: %f p0: %f %f p1: %f %f t: %f %f dz: %f %f\n",ou,oldu,p0.real,p0.imag,p1.real,p1.imag,t.real,t.imag,dz.real,dz.imag);
	
		 
	}
	(*v)=t;
	return 1;
} 

double evaluate_poly(double *a, double v, int N)
{
	double p0,tt;
	int j;
	

    p0=a[N-1];

	tt=v;
	for(j=N-2;j>0;j--)
    {	
		p0+=tt*a[j];
		tt=v*tt;
	}
	p0+=tt*a[0];
		
	
	return p0;
} 
double complex_evaluate_poly(double *a, complex v, complex *f,int N)
{
	double s,r,p,q,t;
	int j;
	
	p=-2.*v.real;
    q=v.real*v.real+v.imag*v.imag;
    s=0;
    r=a[0];
    
	for(j=1;j<N-1;j++)
        {
		t=a[j]-p*r-s*q;
		s=r;
		r=t;
	}
	f->real=a[N-1]+v.real*r-q*s;
	f->imag=v.imag*r;
	
	return f->real*f->real+f->imag*f->imag;
} 


int svduv(double *d,double *a,double *u,int m,double *v,int n)
{
	double *p,*p1,*q,*pp,*w,*e;
	double s,h,r,t,sv;
	int i,j,k,mm,nm,ms;
	if(m<n) return -1;
	w=(double *)calloc(m+n,sizeof(double)); e=w+m;
	for(i=0,mm=m,nm=n-1,p=a; i<n ;++i,--mm,--nm,p+=n+1){
		if(mm>1){ sv=h=0.;
		for(j=0,q=p,s=0.; j<mm ;++j,q+=n){
			w[j]= *q; s+= *q* *q;
		}
		if(s>0.){
			h=sqrt(s); if(*p<0.) h= -h;
			s+= *p*h; s=1./s; t=1./(w[0]+=h);
			sv=1.+fabs(*p/h);
			for(k=1,ms=n-i; k<ms ;++k){
				for(j=0,q=p+k,r=0.; j<mm ;q+=n) r+=w[j++]* *q;
				r*=s;
				for(j=0,q=p+k; j<mm ;q+=n) *q-=r*w[j++];
			}
			for(j=1,q=p; j<mm ;) *(q+=n)=t*w[j++];
		}
		*p=sv; d[i]= -h;
		}
		if(mm==1) d[i]= *p;
		p1=p+1; sv=h=0.;
		if(nm>1){
			for(j=0,q=p1,s=0.; j<nm ;++j,++q) s+= *q* *q;
			if(s>0.){
				h=sqrt(s); if(*p1<0.) h= -h;
				sv=1.+fabs(*p1/h);
				s+= *p1*h; s=1./s; t=1./(*p1+=h);
				for(k=n,ms=n*(m-i); k<ms ;k+=n){
					for(j=0,q=p1,pp=p1+k,r=0.; j<nm ;++j) r+= *q++ * *pp++;
					r*=s;
					for(j=0,q=p1,pp=p1+k; j<nm ;++j) *pp++ -=r* *q++;
				}
				for(j=1,q=p1+1; j<nm ;++j) *q++ *=t;
			}
			*p1=sv; e[i]= -h;
		}
		if(nm==1) e[i]= *p1;
	}
	ldvmat(a,v,n);
	ldumat(a,u,m,n);
	qrbdv(d,e,u,m,v,n);
	for(i=0; i<n ;++i){
		if(d[i]<=0.){ d[i]= -d[i];
		for(j=0,p=v+i; j<n ;++j,p+=n) *p= - *p;
		}
	}
	free(w);
	return 0;
}
void ldvmat(double *a,double *v,int n)
{ 

double *p0,*q0,*p,*q,*qq;
double h,s;
int i,j,k,mm;
for(i=0,mm=n*n,q=v; i<mm ;++i) *q++ =0.;
*v=1.; q0=v+n*n-1; *q0=1.; q0-=n+1;
p0=a+n*n-n-n-1;
for(i=n-2,mm=1; i>0 ;--i,p0-=n+1,q0-=n+1,++mm){
    if(*(p0-1)!=0.){
		for(j=0,p=p0,h=1.; j<mm ;++j,++p) h+= *p* *p;
		h= *(p0-1); *q0=1.-h;
		for(j=0,q=q0+n,p=p0; j<mm ;++j,q+=n) *q= -h* *p++; 
		for(k=i+1,q=q0+1; k<n ;++k){
			for(j=0,qq=q+n,p=p0,s=0.; j<mm ;++j,qq+=n) s+= *qq* *p++;
			s*=h;
			for(j=0,qq=q+n,p=p0; j<mm ;++j,qq+=n) *qq-=s* *p++;
			*q++ = -s;
		}
	}
    else{
		*q0=1.;
		for(j=0,p=q0+1,q=q0+n; j<mm ;++j,q+=n) *q= *p++ =0.;
	}
}
}

void ldumat(double *a,double *u,int m,int n)
{

double *p0,*q0,*p,*q,*w;
int i,j,k,mm;
double s,h;
w=(double *)calloc(m,sizeof(double));
for(i=0,mm=m*m,q=u; i<mm ;++i) *q++ =0.;
p0=a+n*n-1; q0=u+m*m-1; mm=m-n; i=n-1;
for(j=0; j<mm ;++j,q0-=m+1) *q0=1.;
if(mm==0){ p0-=n+1; *q0=1.; q0-=m+1; --i; ++mm;}
for(; i>=0 ;--i,++mm,p0-=n+1,q0-=m+1){
    if(*p0!=0.){
		for(j=0,p=p0+n,h=1.; j<mm ;p+=n) w[j++]= *p;
		h= *p0; *q0=1.-h;
		for(j=0,q=q0+m; j<mm ;q+=m) *q= -h*w[j++];
		for(k=i+1,q=q0+1; k<m ;++k){
			for(j=0,p=q+m,s=0.; j<mm ;p+=m) s+=w[j++]* *p;
			s*=h;
			for(j=0,p=q+m; j<mm ;p+=m) *p-=s*w[j++];
			*q++ = -s;
		}
	}
    else{
		*q0=1.;
		for(j=0,p=q0+1,q=q0+m; j<mm ;++j,q+=m) *q= *p++ =0.;
	}
}
free(w);
}

int qrbdv(double *dm,double *em,double *um,int mm,double *vm,int m)
{ 
int i,j,k,n,jj,nm;
double u,x,y,a,b,c,s,t,w,*p,*q;
for(j=1,t=fabs(dm[0]); j<m ;++j)
if((s=fabs(dm[j])+fabs(em[j-1]))>t) t=s;
t*=1.e-15; n=100*m; nm=m;
for(j=0; m>1 && j<n ;++j){
    for(k=m-1; k>0 ;--k){
		if(fabs(em[k-1])<t) break;
		if(fabs(dm[k-1])<t){
			for(i=k,s=1.,c=0.; i<m ;++i){
				a=s*em[i-1]; b=dm[i]; em[i-1]*=c;
				dm[i]=u=sqrt(a*a+b*b); s= -a/u; c=b/u;
				for(jj=0,p=um+k-1; jj<mm ;++jj,p+=mm){
					q=p+i-k+1;
					w=c* *p+s* *q; *q=c* *q-s* *p; *p=w;
				}
			}
			break;
		}
	}
    y=dm[k]; x=dm[m-1]; u=em[m-2];
    a=(y+x)*(y-x)-u*u; s=y*em[k]; b=s+s;
    u=sqrt(a*a+b*b);
	if(u!=0.){
		c=sqrt((u+a)/(u+u));
		if(c!=0.) s/=(c*u); else s=1.;
		for(i=k; i<m-1 ;++i){
			b=em[i];
			if(i>k){
				a=s*em[i]; b*=c;
				em[i-1]=u=sqrt(x*x+a*a);
				c=x/u; s=a/u;
			}
			a=c*y+s*b; b=c*b-s*y;
			for(jj=0,p=vm+i; jj<nm ;++jj,p+=nm){
				w=c* *p+s* *(p+1); *(p+1)=c* *(p+1)-s* *p; *p=w;
			}
			s*=dm[i+1]; dm[i]=u=sqrt(a*a+s*s);
			y=c*dm[i+1]; c=a/u; s/=u;
			x=c*b+s*y; y=c*y-s*b;
			for(jj=0,p=um+i; jj<mm ;++jj,p+=mm){
				w=c* *p+s* *(p+1); *(p+1)=c* *(p+1)-s* *p; *p=w;
			}
		}
	}
    em[m-2]=x; dm[m-1]=y;
    if(fabs(x)<t) --m;
    if(m==k+1) --m; 
}
return j;
}
